#!/usr/bin/python3
# -*- coding: utf-8 -*-

from __future__ import print_function
import json
import rospy
from std_msgs.msg import String

import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist

import threading
import time

import sys, select, termios, tty

msg = """
arduino ctrl ROS car
"""

gotNewMsg_flg = False

x = 0
y = 0
z = 0
th = 0
status = 0
speed = 0
turn = 0

# -------------------------------------
def vels(speed,turn):
	return "currently:\tspeed %s\tturn %s " % (speed,turn)
# -------------------------------------
def callback(data):
    global gotNewMsg_flg
    gotNewMsg_flg = True
    rospy.loginfo("msg from arduino: %s", data.data)
    try:
        if data:
            # 解析JSON数据
            parsed_json = json.loads(data.data)
            rospy.loginfo("Parsed JSON: %s", parsed_json)

            # 示例：访问JSON对象中的一个键
            # if 'key_name' in parsed_json:
            #     rospy.loginfo("Value of 'key_name': %s", parsed_json['key_name'])
            global x,y,z,th,speed,turn

            x = int(parsed_json['x'])
            y =  int(parsed_json['y'])
            z =  int(parsed_json['z'])
            th = int(parsed_json['th'])

            speed = float(parsed_json['speed'])
            turn =  float(parsed_json['turn'])


    except json.JSONDecodeError as e:
        rospy.logerr("Failed to parse JSON: %s", e)

# -------------------------------------
# 线程1: 订阅esp32_ctrl topic
def listener():
    # rospy.init_node('teleop_twist_ardunoctrl', anonymous=True)
    # rospy.Subscriber("chatter", String, callback)
    rospy.Subscriber("esp32_ctrl", String, callback)
    rospy.spin()  # Keep the node running until it's stopped
# -------------------------------------

def cmd_vel_publisher():
    # 创建向 cmd_vel 的发送器，将xyz,speed等值通过pub.publish(twist)发送到cmd_vel下
    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
    # rospy.init_node('teleop_twist_ardunoctrl')
    global x,y,z,th,speed,turn
    speed = rospy.get_param("~speed", 0.5)
    turn = rospy.get_param("~turn", 1.0)
    # 这里的代码可以立即运行，不会被上面的rospy.spin()阻塞
    try:
            while not rospy.is_shutdown():
                print(vels(speed,turn))
                # if (status == 14):
                #     print(msg)
                # status = (status + 1) % 15
                global gotNewMsg_flg
                if True == gotNewMsg_flg:
                    twist = Twist()
                    twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
                    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
                    pub.publish(twist)
                gotNewMsg_flg = False
                time.sleep(1)
    except Exception as e:
            print(e)


if __name__ == '__main__':
    rospy.init_node('teleop_twist_ardunoctrl', anonymous=True)

    # 启动一个新线程来运行 rospy.spin()
    t = threading.Thread(target=listener)
    t.start()

    cmd_vel_publisher()
